#!/usr/bin/env python3

"""
无人机导航系统启动文件 - 调试版
包含性能监控、日志记录等调试功能
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, ExecuteProcess
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node
from launch.conditions import IfCondition, UnlessCondition

def generate_launch_description():
    
    # 声明启动参数
    declare_debug_arg = DeclareLaunchArgument(
        'debug',
        default_value='true',
        description='启用调试模式'
    )
    
    declare_performance_monitor_arg = DeclareLaunchArgument(
        'performance_monitor',
        default_value='true',
        description='启用性能监控'
    )
    
    declare_log_level_arg = DeclareLaunchArgument(
        'log_level',
        default_value='info',
        description='日志级别 (debug, info, warn, error)'
    )
    
    declare_rviz_arg = DeclareLaunchArgument(
        'rviz',
        default_value='true',
        description='启动RViz可视化'
    )
    
    declare_config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value='navigation_params.yaml',
        description='配置文件名'
    )
    
    declare_point_lio_arg = DeclareLaunchArgument(
        'use_point_lio',
        default_value='true',
        description='是否使用point_lio定位'
    )
    
    # 获取包目录
    package_dir = os.path.join(
        os.path.dirname(os.path.realpath(__file__)),
        '..'
    )
    config_dir = os.path.join(package_dir, 'config')
    
    # 配置文件路径
    config_file = os.path.join(config_dir, LaunchConfiguration('config_file'))
    
    # 核心导航节点
    navigation_manager_node = Node(
        package='drone_navigation',
        executable='navigation_manager',
        name='navigation_manager',
        output='screen',
        parameters=[config_file],
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
        remappings=[
            ('/cmd_vel', '/drone/cmd_vel'),
            ('/current_pose', '/point_lio/pose'),
            ('/goal_pose', '/move_base_simple/goal')
        ]
    )
    
    # 障碍物检测节点
    obstacle_detector_node = Node(
        package='drone_navigation',
        executable='obstacle_detector',
        name='obstacle_detector',
        output='screen',
        parameters=[config_file],
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
        remappings=[
            ('/livox/lidar', '/livox/lidar'),
            ('/cloud_registered', '/point_lio/cloud_registered')
        ]
    )
    
    # 路径规划节点
    drone_navigator_node = Node(
        package='drone_navigation',
        executable='drone_navigator',
        name='drone_navigator',
        output='screen',
        parameters=[config_file],
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
        remappings=[
            ('/current_pose', '/point_lio/pose'),
            ('/occupancy_grid', '/obstacle_detector/occupancy_grid')
        ]
    )
    
    # 速度控制器节点
    velocity_controller_node = Node(
        package='drone_navigation',
        executable='velocity_controller',
        name='velocity_controller',
        output='screen',
        parameters=[config_file],
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
        remappings=[
            ('/cmd_vel', '/drone/cmd_vel'),
            ('/drone/velocity_output', '/drone/velocity_output')
        ]
    )
    
    # 性能监控节点
    performance_monitor_node = Node(
        package='drone_navigation',
        executable='performance_monitor',
        name='performance_monitor',
        output='screen',
        parameters=[
            {'monitor_interval': 2.0},
            {'log_to_file': True},
            {'warning_cpu_threshold': 80.0},
            {'warning_memory_threshold': 85.0},
            {'warning_temp_threshold': 75.0}
        ],
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
        condition=IfCondition(LaunchConfiguration('performance_monitor'))
    )
    
    # TF静态变换（如果需要）
    static_transform_publisher = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_to_lidar_tf',
        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'livox_frame'],
        condition=IfCondition(LaunchConfiguration('debug'))
    )
    
    # RViz可视化节点
    rviz_config_file = os.path.join(config_dir, 'navigation_rviz.rviz')
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_file],
        output='screen',
        condition=IfCondition(LaunchConfiguration('rviz'))
    )
    
    # 系统诊断进程（启动时运行一次）
    system_diagnostics_process = ExecuteProcess(
        cmd=['python3', os.path.join(package_dir, 'scripts', 'system_diagnostics.py')],
        name='system_diagnostics',
        output='screen',
        condition=IfCondition(LaunchConfiguration('debug'))
    )
    
    # 日志信息
    log_info_start = LogInfo(
        msg=[
            '启动无人机导航系统 (调试版)\n',
            '调试模式: ', LaunchConfiguration('debug'), '\n',
            '性能监控: ', LaunchConfiguration('performance_monitor'), '\n',
            '日志级别: ', LaunchConfiguration('log_level'), '\n',
            '配置文件: ', LaunchConfiguration('config_file'), '\n',
            '='*50
        ]
    )
    
    # point_lio启动（如果需要）
    point_lio_launch = ExecuteProcess(
        cmd=['ros2', 'launch', 'point_lio', 'mapping.launch.py'],
        name='point_lio',
        output='screen',
        condition=IfCondition(LaunchConfiguration('use_point_lio'))
    )
    
    # Livox驱动启动
    livox_driver_launch = ExecuteProcess(
        cmd=['ros2', 'launch', 'livox_ros_driver2', 'msg_MID360_launch.py'],
        name='livox_driver',
        output='screen',
        condition=IfCondition(LaunchConfiguration('debug'))
    )
    
    # 组装启动描述
    return LaunchDescription([
        # 声明参数
        declare_debug_arg,
        declare_performance_monitor_arg,
        declare_log_level_arg,
        declare_rviz_arg,
        declare_config_file_arg,
        declare_point_lio_arg,
        
        # 启动信息
        log_info_start,
        
        # 系统诊断
        system_diagnostics_process,
        
        # 外部依赖
        point_lio_launch,
        livox_driver_launch,
        
        # 核心导航节点
        navigation_manager_node,
        obstacle_detector_node,
        drone_navigator_node,
        velocity_controller_node,
        
        # 监控和调试节点
        performance_monitor_node,
        static_transform_publisher,
        
        # 可视化
        rviz_node,
    ]) 